﻿using System;
using System.Collections.Generic;
using UnityEngine;

using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgcodecsModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.UnityUtils;
using OpenCVForUnity.UnityUtils.Helper;
using OpenCVForUnityExample.DnnModel;
using OpenCVRange = OpenCVForUnity.CoreModule.Range;

public class PoseEstimationController : MonoBehaviour
{
    public bool mask = true;
    
    /// <summary>
    /// The person detector.
    /// </summary>
    internal MediaPipePersonDetector personDetector;
    /// <summary>
    /// The pose estimator.
    /// </summary>
    internal MediaPipePoseEstimator poseEstimator;
    /// <summary>
    /// PERSON_DETECTION_MODEL_FILENAME
    /// </summary>
    protected static readonly string PERSON_DETECTION_MODEL_FILENAME = "OpenCVForUnity/dnn/person_detection_mediapipe_2023mar.onnx";

    /// <summary>
    /// The person detection model filepath.
    /// </summary>
    string person_detection_model_filepath;

    /// <summary>
    /// POSE_ESTIMATION_MODEL_FILENAME
    /// </summary>
    protected static readonly string POSE_ESTIMATION_MODEL_FILENAME = "OpenCVForUnity/dnn/pose_estimation_mediapipe_2023mar.onnx";

    /// <summary>
    /// The pose estimation model filepath.
    /// </summary>
    string pose_estimation_model_filepath;

    private void OnEnable()
    {
        person_detection_model_filepath = Utils.getFilePath(PERSON_DETECTION_MODEL_FILENAME);
        pose_estimation_model_filepath = Utils.getFilePath(POSE_ESTIMATION_MODEL_FILENAME);
        
        Utils.setDebugMode(true);

        if (string.IsNullOrEmpty(person_detection_model_filepath))
        {
            Debug.LogError(PERSON_DETECTION_MODEL_FILENAME +
                           " is not loaded. Please read “StreamingAssets/OpenCVForUnity/dnn/setup_dnn_module.pdf” to make the necessary setup.");
        }
        else
        {
            personDetector = new MediaPipePersonDetector(person_detection_model_filepath, 0.3f, 0.6f,
                5000); // # usually only one person has good performance
        }

        if (string.IsNullOrEmpty(pose_estimation_model_filepath))
        {
            Debug.LogError(POSE_ESTIMATION_MODEL_FILENAME +
                           " is not loaded. Please read “StreamingAssets/OpenCVForUnity/dnn/setup_dnn_module.pdf” to make the necessary setup.");
        }
        else
        {
            poseEstimator = new MediaPipePoseEstimator(pose_estimation_model_filepath, 0.9f);
        }
    }

    public Mat PoseEstimation(Texture2D tex,out Mat poseMat, out Mat outlineMat,bool visualizeMark)
    {
        poseMat = null;
        outlineMat = null;
        
        if (tex == null)
        {
            GameDebug.LogError("tex is null");
            return null;
        }
        Mat img = new Mat(tex.height, tex.width, CvType.CV_8UC3);

        GameDebug.Log($"{tex.name} tex width:{tex.width} height:{tex.height}");
        //Utils.texture2DToMat(tex, img);
        //Debug.LogError($"img cv type:{img.type()}");
        int width = tex.width;
        int height = tex.height;
        // 从纹理中获取像素数据
        Color32[] pixels = tex.GetPixels32();

        // 将像素数据复制到Mat对象
        byte[] pixelBytes = new byte[pixels.Length * 3]; // 每个像素有3个字节（BGR）
        int index = 0;
        for (int y = height - 1; y >= 0; y--)
        {
            for (int x = 0; x < width; x++)
            {
                int i = y * width + x;
                pixelBytes[index++] = pixels[i].b;
                pixelBytes[index++] = pixels[i].g;
                pixelBytes[index++] = pixels[i].r;
            }
        }
        img.put(0, 0, pixelBytes);
        
        if (img.empty())
        {
            img = new Mat(424, 640, CvType.CV_8UC3, new Scalar(0, 0, 0));
            Imgproc.putText(img, "Image is not loaded.", new Point(5, img.rows() - 30),
                Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255, 255), 2, Imgproc.LINE_AA, false);
            Imgproc.putText(img, "Please read console message.", new Point(5, img.rows() - 10),
                Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255, 255), 2, Imgproc.LINE_AA, false);
        }
        else
        {
            TickMeter tm = new TickMeter();
            tm.start();

            Mat persons = personDetector.infer(img);

            tm.stop();
            GameDebug.Log("MediaPipePersonDetector Inference time (preprocess + infer + postprocess), ms: " +
                          tm.getTimeMilli());

            // Estimate the pose of each person
            for (int i = 0; i < persons.rows(); ++i)
            {
                tm.reset();
                tm.start();

                // pose estimator inference
                List<Mat> results = poseEstimator.infer(img, persons.row(i), mask);

                tm.stop();
                GameDebug.Log("MediaPipePoseEstimator Inference time (preprocess + infer + postprocess), ms: " +
                          tm.getTimeMilli());

                poseMat = results[0];
                outlineMat = results[1];
                
                //if (!results[1].empty())
                //poseEstimator.visualize_mask(img, results[1], false,visualizeMark);
                
                if (visualizeMark)
                {
                    poseEstimator.visualize(img, results[0], false, false);
                }
            }
            GameDebug.Log($"{tex.name} img width:{img.cols()} height:{img.rows()} persons.rows():{persons.rows()}");
        }
        return img;
    }
    
    public void GetOutlinePose(Mat poseMat, Mat outlineMat,out OutlineData outlineData,out KeyPoints posePoints)
    {
        outlineData = new OutlineData();
        var outlinePoints = new int[outlineMat.cols(),outlineMat.rows()];
        posePoints = new KeyPoints();
        Vector2Int topPoint = Vector2Int.zero;
        int height = outlineMat.rows();
        
        for (int i = 0; i < outlineMat.rows(); i++)
        {
            for (int j = 0; j < outlineMat.cols(); j++)
            {
                var data = outlineMat.get(i,j);

                if (data[0] > 0)
                {
                    int x = j;
                    int y = height - i - 1;
                    Vector2Int point = new Vector2Int(x, y);
                    
                    outlinePoints[x,y] = 1;
                    
                    if (point.y > topPoint.y)
                        topPoint = point;
                }
            }
        }
        //GameDebug.LogError($"top point:{topPoint}");

        outlineData.allPoints = outlinePoints;
        outlineData.topPoint = topPoint;

        if (poseMat.empty() || poseMat.rows() < 317)
        {
            outlineData = null;
            posePoints = null;
            GameDebug.LogError($"error pose mat rows:{poseMat.rows()}");
            return;
        }
        
        int auxiliary_points_num = 6;
        Mat results_col4_199_39x5 = poseMat.rowRange(new OpenCVRange(4, 199 - (5 * auxiliary_points_num))).reshape(1, 39 - auxiliary_points_num);
        float[] landmarks_screen_xy = new float[(39 - auxiliary_points_num) * 2];
        results_col4_199_39x5.colRange(new OpenCVRange(0, 2)).get(0, 0, landmarks_screen_xy);

        float[] landmarks_screen_xyz = new float[(39 - auxiliary_points_num) * 3];
        results_col4_199_39x5.colRange(new OpenCVRange(0, 3)).get(0, 0, landmarks_screen_xyz);

        // # only show visible keypoints which presence bigger than 0.8
        float[] landmarks_presence = new float[(39 - auxiliary_points_num)];
        results_col4_199_39x5.colRange(new OpenCVRange(4, 5)).get(0, 0, landmarks_presence);
            
        Mat results_col199_316_39x3 = poseMat.rowRange(new OpenCVRange(199, 316 - (3 * auxiliary_points_num))).reshape(1, 39 - auxiliary_points_num);
        float[] landmarks_world = new float[(39 - auxiliary_points_num) * 3];
        results_col199_316_39x3.get(0, 0, landmarks_world);
            
        //draw pose key points
        int leftShoulderPoint = 11;
        int rightShoulderPoint = 12;
        int leftElbowPoint = 13;
        int rightElbowPoint = 14;
        int leftWristPoint = 15;
        int rightWristPoint = 16;
        int leftThighPoint = 23;
        int leftCalfPoint = 25;
        int leftAnklePoint = 27;
        int leftFootPoint = 29;
        int leftToePoint = 31;
        int rightThighPoint = 24;
        int rightCalfPoint = 26;
        int rightAnklePoint = 28;
        int rightFootPoint = 30;
        int rightToePoint = 32;
        
        for (int j = 0; j < landmarks_screen_xyz.Length / 3; ++j)
        {
            int idx = j * 3;
            if (landmarks_presence[j] > 0.8)
            {
                var pos = new Vector2(landmarks_screen_xyz[idx], height - landmarks_screen_xyz[idx + 1] - 1);
                if (j == leftShoulderPoint) posePoints.leftShoulderPoint = pos;
                if (j == rightShoulderPoint) posePoints.rightShoulderPoint = pos;
                if (j == leftElbowPoint) posePoints.leftElbowPoint = pos;
                if (j == rightElbowPoint) posePoints.rightElbowPoint = pos;
                if (j == leftWristPoint) posePoints.leftWristPoint = pos;
                if (j == rightWristPoint) posePoints.rightWristPoint = pos;
                if (j == leftThighPoint) posePoints.leftThighPoint = pos;
                if (j == leftCalfPoint) posePoints.leftCalfPoin = pos;
                if (j == leftAnklePoint) posePoints.leftAnklePoint = pos;
                if (j == leftFootPoint) posePoints.leftFootPoint = pos;
                if (j == leftToePoint) posePoints.leftToePoint = pos;
                if (j == rightThighPoint) posePoints.rightThighPoint = pos;
                if (j == rightCalfPoint) posePoints.rightCalfPoint = pos;
                if (j == rightAnklePoint) posePoints.rightAnklePoint = pos;
                if (j == rightFootPoint) posePoints.rightFootPoint = pos;
                if (j == rightToePoint) posePoints.rightToePoint = pos;
                //posePoints.Add(pos);
            }
        }
    }
    

    public static Texture2D Mat2Tex(Mat img)
    {
        Texture2D texture = new Texture2D(img.cols(), img.rows(), TextureFormat.RGB24, false);
        Utils.matToTexture2D(img, texture);
        return texture;
    }
    
    
    public FrontData GetFrontData(OutlineData frontOutline, KeyPoints posePoints)
    {
        var outlinePoints = frontOutline.allPoints;
        Vector2 footPoint = (posePoints.leftFootPoint + posePoints.leftToePoint+ posePoints.rightFootPoint + posePoints.rightToePoint) / 4;
        Vector2 downPoint = new Vector2(int.MaxValue, int.MaxValue);
        int heightLineCol = (int)footPoint.x;
        int leftFootPointCol = (int)posePoints.leftFootPoint.x;
        int rightFootPointCol = (int)posePoints.rightFootPoint.x;
        Vector2Int topPoint = frontOutline.topPoint;
        /*Vector2Int topPoint = Vector2Int.zero;
        //GameDebug.LogError($"footPointCol:{footPointCol}  ");
        for (int row = 0; row < outlinePoints.GetLength(1); row++)
        {
            if (outlinePoints[heightLineCol, row] > 0)
            {
                var pos = new Vector2Int(heightLineCol, row);
                if (pos.y > topPoint.y) 
                    topPoint = pos;
            }
            /*if (outlinePoints[leftFootPointCol, row] > 0)
            {
                var pos = new Vector2Int(leftFootPointCol, row);
                if (pos.y < downPoint.y) 
                    downPoint = pos;
            }
            if (outlinePoints[rightFootPointCol, row] > 0)
            {
                var pos = new Vector2Int(rightFootPointCol, row);
                if (pos.y < downPoint.y) 
                    downPoint = pos;
            }#1#
        }*/
        //footPoint.y = (int)(footPoint.y + (downPoint.y - footPoint.y) * 0.25f);
        frontOutline.topPoint = topPoint;
        float tall = topPoint.y - footPoint.y;

        //检测肩宽
        int shouldCheckY = (int)((posePoints.leftShoulderPoint.y + posePoints.rightShoulderPoint.y) / 2 + (0.01f * tall));
        
        float shoulderCheckRate = 0.03f;
        int shoulderCheckCount = (int)(shoulderCheckRate * tall);
        float shoulderWidth = 0;
        Vector2 shoulderLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
        Vector2 shoulderRightPoint = Vector2.zero;

        Vector2 highShoulderPoint = posePoints.leftShoulderPoint.y > posePoints.rightShoulderPoint.y
            ? posePoints.leftShoulderPoint
            : posePoints.rightShoulderPoint;
        
        int checkShoulderBeginY = (int)(highShoulderPoint.y);

        /*
        int beginCol = (int)highShoulderPoint.x;
        int upEdgeRow = 0;
        for (int row = checkShoulderBeginY; row <= checkShoulderBeginY + shoulderCheckCount; row++)
        {
            if (outlinePoints[beginCol, row] <= 0)
            {
                upEdgeRow = row;
                break;
            }
        }
        */
        /*int shoulderCheckRow = checkShoulderBeginY;//(checkShoulderBeginY + upEdgeRow) / 2;
        var curShoulderLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
        var curShoulderRightPoint = Vector2.zero;

        for (int col = 0; col < outlinePoints.GetLength(0); col++)
        {
            if (outlinePoints[col, shoulderCheckRow] > 0)
            {
                var posInt = new Vector2Int(col, shoulderCheckRow);
                var pos = new Vector2(posInt.x, posInt.y);

                if (pos.x < curShoulderLeftPoint.x)
                {
                    curShoulderLeftPoint = pos;
                }

                if (pos.x > curShoulderRightPoint.x)
                {
                    curShoulderRightPoint = pos;
                }
            }
        }
        shoulderWidth = Mathf.Abs(curShoulderRightPoint.x - curShoulderLeftPoint.x);;
        shoulderLeftPoint = curShoulderLeftPoint;
        shoulderRightPoint = curShoulderRightPoint;*/
        
        for (int row = checkShoulderBeginY; row <= checkShoulderBeginY + shoulderCheckCount; row++)
        {
            var curShoulderLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curShoulderRightPoint = Vector2.zero;
            float chestWidthOnElbowY = 0;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    if (pos.x < curShoulderLeftPoint.x)
                    {
                        curShoulderLeftPoint = pos;
                    }

                    if (pos.x > curShoulderRightPoint.x)
                    {
                        curShoulderRightPoint = pos;
                    }
                }
            }
            float curShoulerWidth =  Mathf.Abs(curShoulderRightPoint.x - curShoulderLeftPoint.x);
            if (shoulderWidth<= 0 || (shoulderWidth - curShoulerWidth) < 3.5f)
            {
                shoulderWidth = curShoulerWidth;
                shoulderLeftPoint = curShoulderLeftPoint;
                shoulderRightPoint = curShoulderRightPoint;
            }
        }

        //检测胸宽
        float chestCheckRate = 0.08f;
        int chestCheckCount = (int)(chestCheckRate * tall);
        float chestWidth = 0;
        Vector2 chestLeftPoint = Vector2.zero;
        Vector2 chestRightPoint = Vector2.zero;
        Vector2 chestCenterPoint = (posePoints.leftElbowPoint + posePoints.rightElbowPoint) / 2;
        Vector2Int chestCenterPointInt = new Vector2Int((int)chestCenterPoint.x, (int)chestCenterPoint.y);
        int chestCheckBeginRow = (int)((posePoints.leftElbowPoint.y + posePoints.rightElbowPoint.y)/2);
        for (int row = chestCheckBeginRow; row <= chestCheckBeginRow + chestCheckCount; row++)
        {
            var curChestLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curChestRightPoint = Vector2.zero;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    bool connectToArm = false;
                    if (posInt.x > chestCenterPointInt.x)
                    {
                        Vector2Int leftElbowInt = new Vector2Int((int)posePoints.leftElbowPoint.x,
                            (int)posePoints.leftElbowPoint.y);
                        connectToArm = ConnectToPoint(outlinePoints, posInt, leftElbowInt);
                    }
                    else
                    {
                        Vector2Int rightElbowInt = new Vector2Int((int)posePoints.rightElbowPoint.x,
                            (int)posePoints.rightElbowPoint.y);
                        connectToArm = ConnectToPoint(outlinePoints, posInt, rightElbowInt);
                    }

                    if (ConnectToPoint(outlinePoints, posInt, chestCenterPointInt) && !connectToArm)
                    {
                        if (pos.x < curChestLeftPoint.x)
                        {
                            curChestLeftPoint = pos;
                        }
                        if (pos.x > curChestRightPoint.x)
                        {
                            curChestRightPoint = pos;
                        }

                        Gizmos.color = Color.yellow;
                    }
                    else
                    {
                        Gizmos.color = Color.red;
                    }
                }
            }
            float curChestWidth = Mathf.Abs(curChestRightPoint.x - curChestLeftPoint.x);
            if (chestWidth < curChestWidth)
            {
                float checkThreshold = 0.1f * Mathf.Abs(posePoints.leftShoulderPoint.x - posePoints.rightShoulderPoint.x);
                if (chestWidth <= 0 || (curChestWidth - chestWidth) < checkThreshold)
                {
                    chestWidth = curChestWidth;
                    chestLeftPoint = curChestLeftPoint;
                    chestRightPoint = curChestRightPoint;
                }
            }
        }

        //检测臀宽
        float hipCheckRate = 0.03f;
        int hipCheckCount = (int)(hipCheckRate * tall);
        float hipWidth = 0;
        Vector2 hipLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
        Vector2 hipRightPoint = Vector2.zero;
        int checkHipBeginY = (int)posePoints.leftThighPoint.y;
        Vector2 hipCenterPoint = (posePoints.leftThighPoint + posePoints.rightThighPoint) / 2;
        Vector2Int hipCenterPointInt = new Vector2Int((int)hipCenterPoint.x, (int)hipCenterPoint.y);
        for (int row = checkHipBeginY - hipCheckCount; row <= checkHipBeginY + hipCheckCount; row++)
        {
            var curHipLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curHipRightPoint = Vector2.zero;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    if (ConnectToPoint(outlinePoints, posInt, hipCenterPointInt))
                    {
                        if (pos.x < curHipLeftPoint.x)
                        {
                            curHipLeftPoint = pos;
                        }

                        if (pos.x > curHipRightPoint.x)
                        {
                            curHipRightPoint = pos;
                        }
                    }

                    //pos = TransformShowPoint(pos);
                    //DrawRect(pos);
                }
            }

            float curHipWidth = curHipRightPoint.x - curHipLeftPoint.x;
            if (hipWidth < curHipWidth)
            {
                hipWidth = curHipWidth;
                hipLeftPoint = curHipLeftPoint;
                hipRightPoint = curHipRightPoint;
            }
        }

        //检测腰宽
        float WaistCheckRate = 0.03f;
        int WaistCheckCount = (int)(WaistCheckRate * tall);
        float waistWidth = 0;
        Vector2 WaistLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
        Vector2 WaistRightPoint = Vector2.zero;
        int checkWaistBeginY = (checkHipBeginY + checkShoulderBeginY) / 2;
        for (int row = checkWaistBeginY - WaistCheckCount; row <= checkWaistBeginY + WaistCheckCount; row++)
        {
            var curWaistLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curWaistRightPoint = Vector2.zero;
            float chestWidthOnElbowY = 0;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    bool connectToArm = false;
                    if (posInt.x > chestCenterPointInt.x)
                    {
                        Vector2Int leftElbowInt = new Vector2Int((int)posePoints.leftElbowPoint.x,
                            (int)posePoints.leftElbowPoint.y);
                        connectToArm = ConnectToPoint(outlinePoints, posInt, leftElbowInt);
                    }
                    else
                    {
                        Vector2Int rightElbowInt = new Vector2Int((int)posePoints.rightElbowPoint.x,
                            (int)posePoints.rightElbowPoint.y);
                        connectToArm = ConnectToPoint(outlinePoints, posInt, rightElbowInt);
                    }

                    if (ConnectToPoint(outlinePoints, posInt, chestCenterPointInt) && !connectToArm)
                    {
                        if (pos.x < curWaistLeftPoint.x)
                        {
                            curWaistLeftPoint = pos;
                        }

                        if (pos.x > curWaistRightPoint.x)
                        {
                            curWaistRightPoint = pos;
                        }
                    }

                    //Gizmos.color = Color.magenta;
                    //pos = TransformShowPoint(pos);
                    //DrawRect(pos);
                }
            }

            float curWaistWidth = curWaistRightPoint.x - curWaistLeftPoint.x;
            float checkThreshold = 0.1f * Mathf.Abs(posePoints.leftShoulderPoint.x - posePoints.rightShoulderPoint.x);
            if (waistWidth <= 0 || (waistWidth > curWaistWidth && waistWidth - curWaistWidth < checkThreshold))
            {
                waistWidth = curWaistWidth;
                WaistLeftPoint = curWaistLeftPoint;
                WaistRightPoint = curWaistRightPoint;
            }
        }
    

        FrontData data = new FrontData();
        data.tall = tall;
        data.footPointY = footPoint.y;
        data.topPointY = topPoint.y;
        data.backShouldLeftPoint = shoulderLeftPoint;
        data.backShouldRightPoint = shoulderRightPoint;
        data.chestLeftPoint = chestLeftPoint + new Vector2(3,0);
        data.chestRightPoint = chestRightPoint+ new Vector2(-1,0);
        data.waistLeftPoint = WaistLeftPoint + new Vector2(1,0);
        data.waistRightPoint = WaistRightPoint+ new Vector2(-1,0);
        data.hipLeftPoint = hipLeftPoint + new Vector2(2,0);
        data.hipRightPoint = hipRightPoint+ new Vector2(-1,0);
        //GameDebug.Log($"front data:{data}");
        return data;
    }
    public SideData GetSideData(FrontData frontData,OutlineData sideOutline, KeyPoints sidePose)
    {
        var outlinePoints = sideOutline.allPoints;
        
        
        Vector2Int topPoint = sideOutline.topPoint;
        //Vector2Int topPoint = Vector2Int.zero;
        Vector2 footPoint = (sidePose.leftFootPoint + sidePose.rightFootPoint) / 2;
        float rate2Front = 1;
        footPoint.y = topPoint.y - frontData.tall * rate2Front;
        /*Vector2Int footPoint = new Vector2Int(int.MaxValue,int.MaxValue);
        
        int heightLineCol = (int)((sidePose.leftThighPoint + sidePose.rightThighPoint) / 2).x;
        int footLineCol = (int)(sidePose.leftFootPoint.x);
        int ankleLineCol = (int)(sidePose.leftAnklePoint.x);
        for (int r = 0; r < outlinePoints.GetLength(1); r++)
        {
            /*if (outlinePoints[heightLineCol, r] > 0)
            {
                var pos = new Vector2Int(heightLineCol, r);
                if (pos.y > topPoint.y) 
                    topPoint = pos;
            }  #1#
            if (outlinePoints[footLineCol, r] > 0)
            {
                var pos = new Vector2Int(footLineCol, r);
                if (pos.y < footPoint.y)
                    footPoint = pos;
            }
            if (outlinePoints[ankleLineCol, r] > 0)
            {
                var pos = new Vector2Int(ankleLineCol, r);
                if (pos.y < footPoint.y)
                    footPoint = pos;
            }
        }
        //sideOutline.topPoint = topPoint;
        footPoint.y += (int)(Mathf.Abs(sidePose.leftFootPoint.y - sidePose.rightFootPoint.y)/2);*/
        
        float tall = topPoint.y - footPoint.y;

        float chestPosYRate = (frontData.chestLeftPoint.y - frontData.footPointY) / frontData.tall;
        int chestPosY = (int)(tall * chestPosYRate + footPoint.y);
        float chestDepth = 0;
        Vector2 chestLeftPoint = Vector2.zero;
        Vector2 chestRightPoint = Vector2.zero;
        int row = chestPosY;
        if (row < 0 || row >= outlinePoints.GetLength(1))
        {
            GameDebug.LogError($"error chesi posY:{row}");  
        }
        else
        {
            var curChestLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curChestRightPoint = Vector2.zero;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    if (pos.x < curChestLeftPoint.x)
                    {
                        curChestLeftPoint = pos;
                    }
                    if (pos.x > curChestRightPoint.x)
                    {
                        curChestRightPoint = pos;
                    }
                }
            }
            float curChestWidth = curChestRightPoint.x - curChestLeftPoint.x;
            if (chestDepth < curChestWidth)
            {
                chestDepth = curChestWidth;
                chestLeftPoint = curChestLeftPoint;
                chestRightPoint = curChestRightPoint;
            }
        }
        
        float WaistPosYRate = (frontData.waistLeftPoint.y - frontData.footPointY) / frontData.tall;
        //GameDebug.Log($"tall:{tall} frontData tall:{frontData.tall} frontData.waistLeftPoint.y:{frontData.waistLeftPoint.y} frontData.footPointY:{frontData.footPointY} WaistPosYRate:{WaistPosYRate}");
        int WaistPosY = (int)(tall * WaistPosYRate + footPoint.y);
        float WaistDepth = 0;
        Vector2 WaistLeftPoint = Vector2.zero;
        Vector2 WaistRightPoint = Vector2.zero;
        row = WaistPosY;
        if (row < 0 || row >= outlinePoints.GetLength(1))
        {
            GameDebug.LogError($"error waist posY:{row}");  
            return null;
        }
        else
        {
            var curWaistLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curWaistRightPoint = Vector2.zero;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    if (pos.x < curWaistLeftPoint.x)
                    {
                        curWaistLeftPoint = pos;
                    }
                    if (pos.x > curWaistRightPoint.x)
                    {
                        curWaistRightPoint = pos;
                    }
                }
            }
            float curWaistWidth = curWaistRightPoint.x - curWaistLeftPoint.x;
            if (WaistDepth < curWaistWidth)
            {
                WaistDepth = curWaistWidth;
                WaistLeftPoint = curWaistLeftPoint;
                WaistRightPoint = curWaistRightPoint;
            }
        }
        
        float HipPosYRate = (frontData.hipLeftPoint.y - frontData.footPointY) / frontData.tall;
        int HipPosY = (int)(tall * HipPosYRate + footPoint.y);
        float HipDepth = 0;
        Vector2 HipLeftPoint = Vector2.zero;
        Vector2 HipRightPoint = Vector2.zero;
        row = HipPosY;
        if (row < 0 || row >= outlinePoints.GetLength(1))
        {
            GameDebug.LogError($"error hip posY:{row}");
            return null;
        }
        else
        {
            var curHipLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
            var curHipRightPoint = Vector2.zero;

            for (int col = 0; col < outlinePoints.GetLength(0); col++)
            {
                if (outlinePoints[col, row] > 0)
                {
                    var posInt = new Vector2Int(col, row);
                    var pos = new Vector2(posInt.x, posInt.y);

                    if (pos.x < curHipLeftPoint.x)
                    {
                        curHipLeftPoint = pos;
                    }
                    if (pos.x > curHipRightPoint.x)
                    {
                        curHipRightPoint = pos;
                    }
                }
            }
            float curHipWidth = curHipRightPoint.x - curHipLeftPoint.x;
            if (HipDepth < curHipWidth)
            {
                HipDepth = curHipWidth;
                HipLeftPoint = curHipLeftPoint;
                HipRightPoint = curHipRightPoint;
            }
        }
        //GameDebug.Log($"frontTall:{frontData.tall}  side tall:{tall}");
        
        SideData data = new SideData();
        data.tall = tall;
        data.footPointY = footPoint.y;
        data.topPointY = topPoint.y;
        data.chestLeftPoint = chestLeftPoint + new Vector2(1,0);
        data.chestRightPoint = chestRightPoint + new Vector2(-1,0);
        data.waistLeftPoint = WaistLeftPoint + new Vector2(1,0);
        data.waistRightPoint = WaistRightPoint + new Vector2(-1,0);
        data.hipLeftPoint = HipLeftPoint + new Vector2(2,0);
        data.hipRightPoint = HipRightPoint + new Vector2(-1,0);
        return data;
    }

    bool ConnectToPoint(int[,]outlinePoints,Vector2Int from, Vector2Int to)
    {
        bool connected = true;

        int curX = from.x;
        int curY = from.y;
        int tryCount = 0;
        while (curX != to.x || curY != to.y)
        {
            int difX = to.x - curX;
            int difY = to.y - curY;

            int distanceX = Mathf.Abs(difX);
            int distanceY = Mathf.Abs(difY);

            int stepX = 0;
            int stepY = 0;
            if (distanceX > distanceY)
            {
                stepX = difX / distanceX;
                stepY = difY / distanceX;
            }
            else
            {
                stepX = difX / distanceY;
                stepY = difY / distanceY;
            }
            curX += stepX;
            curY += stepY;

            if (outlinePoints[curX, curY] <= 0)
            {
                connected = false;
                break;
            }

            tryCount++;
            if (tryCount > 5000)
            {
                GameDebug.LogError($"try too many from:{from} to:{to}");
                break;
            }
        }

        return connected;
    }
}